#include <kernel/kobject.h>
#include <kernel/initcall.h>
#include <kernel/syslog.h>
#include <mm/mem.h>
#include <drivers/platform.h>
#include <drivers/spi.h>
#include <rp2040.h>
#include <kernel/fdt.h>
#include <drivers/clock.h>

#include "rp2040_spi.h"
#include "hardware/rp2040_spi.h"
#include "hardware/rp2040_clocks.h"


enum spi_mode_e {
    SPIDEV_MODE0 = 0,     /* CPOL=0 CHPHA=0 */
    SPIDEV_MODE1,         /* CPOL=0 CHPHA=1 */
    SPIDEV_MODE2,         /* CPOL=1 CHPHA=0 */
    SPIDEV_MODE3,         /* CPOL=1 CHPHA=1 */
    SPIDEV_MODETI,        /* CPOL=0 CPHA=1 TI Synchronous Serial Frame Format */
  };


#define getreg8(a)     (*(volatile uint8_t *)(a))
#define putreg8(v,a)   (*(volatile uint8_t *)(a) = (v))
#define getreg16(a)    (*(volatile uint16_t *)(a))
#define putreg16(v,a)  (*(volatile uint16_t *)(a) = (v))
#define getreg32(a)    (*(volatile uint32_t *)(a))
#define putreg32(v,a)  (*(volatile uint32_t *)(a) = (v))
#define getreg64(a)    (*(volatile uint64_t *)(a))
#define putreg64(v,a)  (*(volatile uint64_t *)(a) = (v))

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

/* 8 frame FIFOs for both transmit and receive */

#define RP2040_SPI_FIFOSZ        8

/****************************************************************************
 * Private Types
 ****************************************************************************/

/* This structure describes the state of the SPI driver */

struct rp2040_spidev_s
{
  uint32_t         spibase;     /* SPIn base address */
  uint32_t         spibasefreq;
  uint32_t         frequency;   /* Requested clock frequency */
  uint32_t         actual;      /* Actual clock frequency */
  uint8_t          nbits;       /* Width of word in bits (4 to 16) */
  uint8_t          mode;        /* Mode 0,1,2,3 */
  uint8_t          port;        /* Port number */
  int              initialized; /* Initialized flag */
};

/****************************************************************************
 * Private Data
 ****************************************************************************/
static struct rp2040_spidev_s g_spi0dev = {
  .spibase           = RP2040_SPI0_BASE,
  .spibasefreq       = 0,
  .port              = 0,
  .initialized       = 0,
};

static inline uint32_t spi_getreg(struct rp2040_spidev_s *dev, uint32_t offset)
{
  return getreg32(dev->spibase + (uint32_t)offset);
}

static inline void spi_putreg(struct rp2040_spidev_s *dev, uint32_t offset, uint32_t value)
{
  putreg32(value, dev->spibase + (uint32_t)offset);
}

uint32_t rp2040_spi_setfrequency(uint32_t frequency)
{
  uint32_t divisor;
  uint32_t actual;

  struct rp2040_spidev_s *dev = &g_spi0dev;

  /* frequency = SPI_CLOCK / divisor, or divisor = SPI_CLOCK / frequency */

  dev->spibasefreq = 228000000;
  divisor = dev->spibasefreq / frequency;

  /* "In master mode, CPSDVSRmin = 2 or larger (even numbers only)" */

  if (divisor < 2)
    {
      divisor = 2;
    }
  else if (divisor > 254)
    {
      divisor = 254;
    }

  divisor = (divisor + 1) & ~1;

  /* Save the new divisor value */

  spi_putreg(dev, RP2040_SPI_SSPCPSR_OFFSET, 2);

  /* Calculate the new actual */

  actual = dev->spibasefreq / divisor;

  /* Save the frequency setting */

  dev->frequency = frequency;
  dev->actual    = actual;

  return actual;
}

void rp2040_spi_setmode(enum spi_mode_e mode)
{
  uint32_t regval;

  struct rp2040_spidev_s *dev = &g_spi0dev;
  /* Has the mode changed? */

  if (mode != dev->mode)
    {
      /* Yes... Set CR0 appropriately */

      regval = spi_getreg(dev, RP2040_SPI_SSPCR0_OFFSET);
      regval &= ~(RP2040_SPI_SSPCR0_SPO | RP2040_SPI_SSPCR0_SPH);

      switch (mode)
        {
          case SPIDEV_MODE0: /* CPOL=0; CPHA=0 */
            break;

          case SPIDEV_MODE1: /* CPOL=0; CPHA=1 */
            regval |= RP2040_SPI_SSPCR0_SPH;
            break;

          case SPIDEV_MODE2: /* CPOL=1; CPHA=0 */
            regval |= RP2040_SPI_SSPCR0_SPO;
            break;

          case SPIDEV_MODE3: /* CPOL=1; CPHA=1 */
            regval |= (RP2040_SPI_SSPCR0_SPO | RP2040_SPI_SSPCR0_SPH);
            break;

          default:
            return;
        }

      spi_putreg(dev, RP2040_SPI_SSPCR0_OFFSET, regval);

      /* Save the mode so that subsequent re-configurations will be faster */

      dev->mode = mode;
    }
}
void rp2040_spi_setbits(int nbits)
{
  uint32_t regval;

  struct rp2040_spidev_s *dev = &g_spi0dev;
  /* Has the number of bits changed? */

  if (nbits != dev->nbits)
    {
      /* Yes... Set CR0 appropriately */

      regval = spi_getreg(dev, RP2040_SPI_SSPCR0_OFFSET);
      regval &= ~RP2040_SPI_SSPCR0_DSS_MASK;
      regval |= ((nbits - 1) << RP2040_SPI_SSPCR0_DSS_SHIFT);
      spi_putreg(dev, RP2040_SPI_SSPCR0_OFFSET, regval);

      /* Save the selection so that re-configurations will be faster
       */

      dev->nbits = nbits;
    }
}

/****************************************************************************
 * Name: spi_send
 *
 * Description:
 *   Exchange one word on SPI
 *
 * Input Parameters:
 *   dev - Device-specific state data
 *   wd  - The word to send.  the size of the data is determined by the
 *         number of bits selected for the SPI interface.
 *
 * Returned Value:
 *   response
 *
 ****************************************************************************/

uint32_t rp2040_spi_send(uint32_t wd)
{
  register uint32_t regval;

  struct rp2040_spidev_s *dev = &g_spi0dev;
 
  while (!(spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_TNF))
    ;

  /* Write the byte to the TX FIFO */

  spi_putreg(dev, RP2040_SPI_SSPDR_OFFSET, wd);

  /* Wait for the RX FIFO not empty */

  while (!(spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_RNE))
    ;

  /* Get the value from the RX FIFO and return it */

  regval = spi_getreg(dev, RP2040_SPI_SSPDR_OFFSET);

  return regval;
}

void rp2040_spi_exchange(const void *txbuffer, void *rxbuffer, size_t nwords)
{
  union
  {
    const uint8_t *p8;
    const uint16_t *p16;
    const void *pv;
  } tx;

  union
  {
    uint8_t *p8;
    uint16_t *p16;
    void *pv;
  } rx;

  struct rp2040_spidev_s *dev = &g_spi0dev;
  uint32_t data;
  uint32_t datadummy = (dev->nbits > 8) ? 0xffff : 0xff;
  uint32_t rxpending = 0;

  tx.pv = txbuffer;
  rx.pv = rxbuffer;

  //gpio_set(17, 0);
  while (nwords || rxpending)
    {
      /* Write data to the data register while (1) the TX FIFO is
       * not full, (2) we have not exceeded the depth of the TX FIFO,
       * and (3) there are more bytes to be sent.
       */

      //pl_syslog_info("TX: rxpending: %d, PRId32 nwords: %d\n", rxpending, nwords);
      while ((spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) &
              RP2040_SPI_SSPSR_TNF) &&
             (rxpending < RP2040_SPI_FIFOSZ) && nwords)
        {
          if (txbuffer)
            {
              if (dev->nbits > 8)
                {
                  data = (uint32_t)*tx.p16++;
                }
              else
                {
                  data = (uint32_t)*tx.p8++;
                }
            }

          spi_putreg(dev, RP2040_SPI_SSPDR_OFFSET,
                     txbuffer ? data : datadummy);
          nwords--;
          rxpending++;
        }

      /* Now, read the RX data from the RX FIFO
       * while the RX FIFO is not empty
       */

      //pl_syslog_info("RX: rxpending: %d", rxpending);
      while (spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) &
             RP2040_SPI_SSPSR_RNE)
        {
          data = spi_getreg(dev, RP2040_SPI_SSPDR_OFFSET);
          if (rxbuffer)
            {
              if (dev->nbits > 8)
                {
                  *rx.p16++ = (uint16_t)data;
                }
              else
                {
                  *rx.p8++ = (uint8_t)data;
                }
            }

          rxpending--;
        }
    }
	//gpio_set(17, 1);
}

int rp2040_spibus_initialize(int port)
{
  struct rp2040_spidev_s *priv;
  uint32_t regval;
  int i;

  if (port > 1) {
    return -1;
  }

  priv = &g_spi0dev;

  if (priv->initialized) {
    return -1;
  }

  putreg32(RP2040_CLOCKS_ENABLED0_clk_peri_spi0 | RP2040_CLOCKS_ENABLED0_clk_sys_spi0, RP2040_CLOCKS_ENABLED0);
  
  /* Configure clocking */
  priv->spibasefreq = 133 * 1000000;

  /* Configure 8-bit SPI mode */
  spi_putreg(priv, RP2040_SPI_SSPCR0_OFFSET,
             ((8 - 1) << RP2040_SPI_SSPCR0_DSS_SHIFT) |
             (0 << RP2040_SPI_SSPCR0_FRF_SHIFT));

  /* Disable SPI and all interrupts (we'll poll for all data) */
  spi_putreg(priv, RP2040_SPI_SSPCR1_OFFSET, 0);
  spi_putreg(priv, RP2040_SPI_SSPIMSC_OFFSET, 0);

  /* Clear interrupts */
  spi_putreg(priv, RP2040_SPI_SSPICR_OFFSET, 0x3);

  /* Set the initial SPI configuration */
  priv->frequency = 32000000;
  priv->nbits     = 8;
  priv->mode      = SPIDEV_MODE0;

  /* Select a default frequency of approx. 400KHz */
  rp2040_spi_setfrequency(32000000);
  rp2040_spi_setbits(8);
  rp2040_spi_setmode(3);
  regval = spi_getreg(priv, RP2040_SPI_SSPCR1_OFFSET);
  spi_putreg(priv, RP2040_SPI_SSPCR1_OFFSET, regval | RP2040_SPI_SSPCR1_SSE);

  for (i = 0; i < RP2040_SPI_FIFOSZ; i++)
    {
      spi_getreg(priv, RP2040_SPI_SSPDR_OFFSET);
    }

  /* Set a initialized flag */

  priv->initialized = 1;
  return 0;
}

/****************************************************************************
 * Name: spi_flush
 *
 * Description:
 *   Flush and discard any words left in the RX fifo.  This can be done
 *   after a device is deselected if you worry about such things.
 *
 * Input Parameters:
 *   dev - Device-specific state data
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void rp2040_spi_flush(int port)
{
  struct rp2040_spidev_s *dev = &g_spi0dev;


  while (!(spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_TNF))
    ;
  spi_putreg(dev, RP2040_SPI_SSPDR_OFFSET, 0xff);

  /* Wait until TX FIFO and TX shift buffer are empty */

  while (spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_BSY);

  /* Wait until RX FIFO is not empty */

  while (!(spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_RNE))
    ;

  /* Then read and discard bytes until the RX FIFO is empty */

  do
    {
      spi_getreg(dev, RP2040_SPI_SSPDR_OFFSET);
    }
  while (spi_getreg(dev, RP2040_SPI_SSPSR_OFFSET) & RP2040_SPI_SSPSR_RNE);
}


static void rp2040_spi_set_cs(struct spi_device *spi, bool enable)
{
    spi_gpio_set_cs(spi, enable);
}


static void rp2040_spi_setup(struct spi_device *spi)
{
  rp2040_spibus_initialize(0);
	rp2040_spi_flush(0);
	rp2040_spi_setbits(8);
  rp2040_spi_setfrequency(32000000);
}


static int rp2040_spi_transfer(struct spi_device *spi, struct spi_transfer *xfer)
{
    uint32_t spi_base = (uint32_t)spi->master->io_base;
    uint8_t *tx_buf = (uint8_t*)xfer->tx_buf;
    uint8_t *rx_buf = (uint8_t*)xfer->rx_buf;
    uint32_t len = xfer->len;

    rp2040_spi_set_cs(spi, true);

    rp2040_spi_exchange(tx_buf, NULL, len);

    rp2040_spi_set_cs(spi, false);
}


static int rp2040_spi_driver_probe(struct platform_device *dev)
{
    fdt_node_t *spi_node= dev->device.dt_node;
    struct spi_gpio gpio_pin;

    const char *spi_clock = fdt_read_prop_string(spi_node, "clock-bus");
    if(spi_clock == NULL) {
        return -1;
    }

    size_t clock_bit = -1;
    if(fdt_read_prop_int(spi_node, "clock-bit", &clock_bit)) {
        PLATFORM_SYSLOG_ERROR("spi: no clock-bit property");
        return -1;
    }

    clock_enable_by_name(spi_clock, clock_bit);

    if(spi_master_get_gpio_from_fdt(spi_node, &gpio_pin)) {
        return -1;
    }
    
    size_t irq_num = 0;
    if(fdt_read_prop_int(spi_node, "interrupts", &irq_num)) {
        PLATFORM_SYSLOG_INFO("spi: no interrupts property");
    }

    struct spi_master *master = kmalloc(sizeof(struct spi_master));
    if(master == NULL) {
        PLATFORM_SYSLOG_ERROR("spi: kmalloc failed");
        return -1;
    }

    master->io_base = dev->io_base;
    master->set_cs = rp2040_spi_set_cs;
    master->setup = rp2040_spi_setup;
    master->transfer = rp2040_spi_transfer;
    master->pin = gpio_pin;

    platform_set_drvdata(dev, master);

    SYSLOG_INFO("spi: send 0xAA");

    fdt_node_t *child =  NULL;
    fdt_for_each_node_child(spi_node, child) {

        struct spi_device *spi_dev = kmalloc(sizeof(struct spi_device));
        if(spi_dev == NULL) {
            PLATFORM_SYSLOG_ERROR("spi: kmalloc failed");
            return -1;
        }

        if(spi_device_get_config_from_fdt(child, spi_dev)) {
            kfree(spi_dev);
            return -1;
        }

        if(spi_device_register(spi_dev)) {
            PLATFORM_SYSLOG_ERROR("spi: spi_device_register failed");
            return -1;
        }

        spi_dev->master = master;

        if(irq_num) {
            spi_dev->irq_num = irq_num;
        }
    }

    uint8_t tx_buf[20] = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE};
    uint8_t rx_buf[20] = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE};

    return 0;
}


static void rp2040_spi_driver_remove(struct platform_device *dev)
{
    
}


static struct of_device_id rp2040_spi_driver_match[] = {
    {.compatible = "rp2040-spi", .data = "rp2040-spi" },
    { }
};


static struct platform_driver rp2040_spi_pdrv = {
    .driver = {
        .name = "rp2040-spi",
        .of_match_table = rp2040_spi_driver_match,
    },
    .probe = rp2040_spi_driver_probe,
    .remove = rp2040_spi_driver_remove,
};


static int rp2040_spi_driver_register(void)
{
    return platform_driver_register(&rp2040_spi_pdrv);
}


driver_initcall(rp2040_spi_driver_register);
